#include <Servo.h> 
#include <string.h>
Servo base,rArm,fArm,claw ;  //建立4个舵机对象
int base_angle=90;
int rArm_angle=90;
int fArm_angle=90;
int claw_angle=33;
bool limit=true;
bool mode=1;
int ServoSteplen=3;



int interval=10;

const int clawMin=33;
const int clawMax=60;
const int baseMin=0;
const int baseMax=180;
const int rArmMin=40;
const int rArmMax=130;
const int fArmMin=30;
const int fArmMax=120;


void setup() 
{ 
  base.attach(11);     // base 伺服舵机连接引脚11 舵机代号'b'
  rArm.attach(10);     // rArm 伺服舵机连接引脚10 舵机代号'r'
  fArm.attach(9);      // fArm 伺服舵机连接引脚9  舵机代号'f'
  claw.attach(6);      // claw 伺服舵机连接引脚6  舵机代号'c'
  Serial.begin(9600);
  base.write(base_angle);
  delay(interval);
  rArm.write(rArm_angle);
  delay(interval);
  fArm.write(fArm_angle);
  delay(interval);
  claw.write(claw_angle);
  delay(interval);
  Serial.print("mode=");
  if(mode==1)
  {
  Serial.println("cmdcontrol");
  }
  else {
  Serial.println("keyboardcontrol");
  }
} 

void loop()
{
 if(Serial.available()>0)
 {
  char choose_cmd =Serial.read();
    if(mode==1)
    {
      cmdcontrol(choose_cmd);
    }
    else if(mode==0) 
    {
      keyboardControl(choose_cmd);
    }
 }
}

void  servo_cmd_hand(char controlcmd,int ServoSteplen)
{

  int Basejointpos,fArmjointpos,rArmjointpos,clawjointpos;
  
  switch (controlcmd) {
    //base底座
    case 'a':
      Serial.println("Basejointpos left move");
      Basejointpos=base.read()+ServoSteplen;
      servocmd('b',Basejointpos);
      break;
    case 'd':
      Serial.println("Basejointpos right move");
      Basejointpos=base.read()-ServoSteplen;
      servocmd('b',Basejointpos);
      break;
    //fArm抬高降落
    case 'w':
      Serial.println("fArmjointpos up move");
      fArmjointpos=fArm.read()+ServoSteplen;
      servocmd('f',fArmjointpos);
      break;
    case 's':
      Serial.println("fArmjointpos down move");
      fArmjointpos=fArm.read()-ServoSteplen;
      servocmd('f',fArmjointpos);
      break;
    //claw开合
    case '4':
      Serial.println("clawjointpos open move");
      clawjointpos=claw.read()+ServoSteplen;
      servocmd('c',clawjointpos);
      break;
    case '6':
      Serial.println("Basejointpos close move");
      clawjointpos=claw.read()-ServoSteplen;
      servocmd('c',clawjointpos);
      break;
    //rArm前进后退
    case '8':
      Serial.println("rArmjointpos forward move");
      rArmjointpos=rArm.read()+ServoSteplen;
      servocmd('r', rArmjointpos);
      break;
    case '5':
      Serial.println("rArmjointpos retreat move");
      rArmjointpos=rArm.read()-ServoSteplen;
      servocmd('r', rArmjointpos);
      break;
    case 'm':
      mode=1;
      Serial.println("cmdcontrol mode has changed: b(base)/r(fArm)/r(rArm)/c(claw) + [number]");
      break;
    case 'o':
      Serial.print("base_angle=");
      Serial.println(base.read());
      Serial.print("rArm_angle=");
      Serial.println(rArm.read());
      Serial.print("fArm_angle=");
      Serial.println(fArm.read());
      Serial.print("claw_angle=");
      Serial.println(claw.read());
      break;
    case 'i':
      reset();
      Serial.println("reset completed");
      break;
    }



}


void reset()//坐标重置指令
{
 int action[4][2]={
  {'b',90},
  {'r',90},
  {'f',90},
  {'c',33}
 };


 for (int step=0;step<sizeof(action) / sizeof(action[0]);step++) {

  servocmd(action[step][0],action[step][1]);
  delay(interval);
 }
}
void cmdcontrol(char choose_cmd)
{
  if(choose_cmd =='c'||choose_cmd=='b'||choose_cmd=='r'||choose_cmd=='f')
  {
    int topos=Serial.parseInt();
    servocmd(choose_cmd,topos);
  }
  else if (choose_cmd == 'a'||choose_cmd == 'd'||choose_cmd == 'w'||choose_cmd == 's'||choose_cmd == '4'||choose_cmd == '5'||choose_cmd == '6'||choose_cmd == '8') 
  {
  Serial.println("指令错误");
  }
  else
  {
   switch (choose_cmd) 
   {
    case 'o':
      Serial.print("base_angle=");
      Serial.println(base.read());
      Serial.print("rArm_angle=");
      Serial.println(rArm.read());
      Serial.print("fArm_angle=");
      Serial.println(fArm.read());
      Serial.print("claw_angle=");
      Serial.println(claw.read());
      break;
    case 'i':
      reset();
      Serial.println("reset completed");
      break;
    case 'm':
      mode=0;
      Serial.println("keyboard mode has changed: a,d(base);w,s(fArm);4,6(claw);5,8(rArm)");
      break;
    }
  }
}
void keyboardControl(char choose_cmd_hand)
{
 servo_cmd_hand(choose_cmd_hand,ServoSteplen);
}




void servocmd(char chosen_servo,int topos)
{
  Serial.print("chosen_servo:");
  Serial.println(chosen_servo);
  Servo servo_now;
  int servo_angle=topos;
  int frompos;
  switch(chosen_servo)
  {
    case 'b':
      if(servo_angle>=baseMin&&servo_angle<=baseMax)
      {
        topos=servo_angle;
        servo_now=base;
        base_angle=servo_angle;
        Serial.print("     base_angle=");
      }
      else {
      limit=false;
      }
      break;
    case 'r':
    if(servo_angle>=rArmMin&&servo_angle<=rArmMax)
    {
      topos=servo_angle;
      servo_now=rArm;
      rArm_angle=servo_angle;
      Serial.print("     rArm_angle=");
    }
    else {
     limit=false;
    }
      break;
    case 'f':
    if(servo_angle>=fArmMin&&servo_angle<=fArmMax)
    {
      topos=servo_angle;
      servo_now=fArm;
      fArm_angle=servo_angle;
      Serial.print("     fArm_angle=");
    }
    else {
     limit=false;
    }
      break;
    case 'c':
    if(servo_angle>=clawMin&&servo_angle<=clawMax)
    {
      topos=servo_angle;
      servo_now=claw;
      claw_angle=servo_angle;
      Serial.print("     claw_angle=");
    }
    else
    {
      limit=false;
    }
      break;
    default:
      Serial.println("don`t have this servo  rewrite,please");
  }
if(limit)
{
frompos=servo_now.read();
if(topos>frompos)
      {
        for(int angle=frompos;angle<=topos;angle++)
        {
        servo_now.write(angle);
  	 	  delay(interval);
        }       
      }
  else
    {
      for(int angle=frompos;angle>=topos;angle--)
        {
        servo_now.write(angle);
  	 	  delay(interval);
        }       
    }
  Serial.println(servo_angle);
}
  else
  {
    limit=true;
    Serial.println("     Limit exceeded");
  }
}
